#include "speed.h"
#include "arm_math.h"
#include "stdio.h"


// 用于计算速度环两次PID的时间差
volatile uint32_t gSpeedPID_TimeCNT;

/**
 * @brief   此函数放在滴答定时器中断中运行，即每次运行+1
 *
 * @param   ms		需要增加的时间（单位ms）
 *
 * @return  none
 */
void SPEED_PID_TIMEBASE(uint16_t ms)
{
	gSpeedPID_TimeCNT += ms;
}

/**
 * @brief   获取此时的计数值
 *
 * @param   none
 *
 * @return  gSpeedPID_TimeCNT
 */
uint32_t SPEED_PID_GETTIME(void)
{
	return gSpeedPID_TimeCNT;
}

/**
 * @brief   判断计数值是否大于上次计数值（用于计算周期时的越限判断）
 *
 * @param   none
 *
 * @return  gSpeedPID_TimeCNT
 */
uint8_t SPEED_PID_TIMEOUT(float timeOut, float startTime)
{
	if((gSpeedPID_TimeCNT - startTime) > timeOut)
		return 1;
	else
		return 0;
}

/* ================================================================ */

/**
 * @brief   速度环PI控制
 *
 * @param   pSpeedPID		pSpeedPID结构体指针
 *
 * @return  none
 */
static void SpeedPIControl(PSPEED_Control_Struct pSpeedPID)
{
//	float p, i, d;
	
	// 1. 实际值
    pSpeedPID->sPID.pre = pSpeedPID->GetMotorSpeed();
	
	/* 调试信息输出（经过变换之后的dq应该是一个常量） */
	SpeedPIDPrintf(pSpeedPID);
	
	// 2. 计算偏差
    pSpeedPID->sPID.bias = pSpeedPID->sPID.tar - pSpeedPID->sPID.pre;
	// 3. 计算pid输出
    
	/* 以下是：ODrive原版的PID算法：增量式(第4步只有下面这一行) */
    pSpeedPID->sPID.out += pSpeedPID->sPID.kp*(pSpeedPID->sPID.bias - pSpeedPID->sPID.lastBias) +  pSpeedPID->sPID.ki * pSpeedPID->sPID.bias;
    /* 以上是：ODrive原版的PID算法：增量式(第4步只有下面这一行) */
	
	/* 以下是：修改后的PID算法：位置式 */
//	// 计算pid的输出
//	p = pSpeedPID->sPID.kp * pSpeedPID->sPID.bias;
//	i = pSpeedPID->sPID.ki * pSpeedPID->sPID.integral;
//	d = pSpeedPID->sPID.kd * (pSpeedPID->sPID.bias - pSpeedPID->sPID.lastBias);
//    // 积分限幅
//	if (i > fabs(pSpeedPID->sPID.outMax))
//	{
//        i = fabs(pSpeedPID->sPID.outMax);
//    }
//    if (i < -fabs(pSpeedPID->sPID.outMax))
//	{
//        i = -fabs(pSpeedPID->sPID.outMax);
//    }
//	// 更新积分
//	pSpeedPID->sPID.integral += i;
//	// 计算输出和
//	pSpeedPID->sPID.out = p + i + d;
	/* 以上是：修改后的PID算法：位置式 */
	
	// 4. 更新误差
	pSpeedPID->sPID.lastBias = pSpeedPID->sPID.bias;
	// 5. 输出限幅
	if (pSpeedPID->sPID.out > pSpeedPID->sPID.outMax)
	{
		pSpeedPID->sPID.out = pSpeedPID->sPID.outMax;
	}
	else if (pSpeedPID->sPID.out < -pSpeedPID->sPID.outMax)
	{
		pSpeedPID->sPID.out = -pSpeedPID->sPID.outMax;
	}
	// 6. 使能判断
    if(pSpeedPID->isEnable == 0)
	{
        pSpeedPID->sPID.out = 0;
    }
	// 7. 设置输出
    pSpeedPID->SetOutCurrent(pSpeedPID->sPID.out);
}

/**
 * @brief   速度环周期控制
 *
 * @param   pSpeedPID		pSpeedPID结构体指针
 *
 * @return  none
 */
void SpeedPIDCycle(PSPEED_Control_Struct pSpeedPID)
{
    if(SPEED_PID_TIMEOUT(pSpeedPID->sPID.cycle * 1000.0f, pSpeedPID->sysTimer))
	{
        pSpeedPID->sysTimer = SPEED_PID_GETTIME();
        SpeedPIControl(pSpeedPID);
    }
}

/**
 * @brief   设置速度环pid参数
 *
 * @param   pSpeedPID		pSpeedPID结构体指针
 * @param   kp				比例
 * @param   ki				积分
 * @param   kd				微分
 *
 * @return  none
 */
void SetSpeedPIDParams(PSPEED_Control_Struct pSpeedPID, float kp, float ki, float kd)
{
    pSpeedPID->sPID.kp = kp;
    pSpeedPID->sPID.ki = ki;
    pSpeedPID->sPID.kd = kd;
}

/**
 * @brief   设置速度环目标值
 *
 * @param   pSpeedPID		pSpeedPID结构体指针
 * @param   tar				速度环目标值
 *
 * @return  none
 */
void SetSpeedPIDTar(PSPEED_Control_Struct pSpeedPID, float tar)
{
    pSpeedPID->sPID.tar = tar;
}

/**
 * @brief   速度环使能
 *
 * @param   pSpeedPID		pSpeedPID结构体指针
 * @param   isEnable		0：失能；1：使能
 *
 * @return  none
 */
void SetSpeedPIDEnable(PSPEED_Control_Struct pSpeedPID, float isEnable)
{
    pSpeedPID->isEnable = isEnable;
}

/**
 * @brief   调试信息输出
 *
 * @param   pSpeedPID		pSpeedPID结构体指针
 *
 * @return  none
 */
#include "vofa.h"
void SpeedPIDPrintf(PSPEED_Control_Struct pSpeedPID)
{
	float ch_data[5];
	
	// 调试信息输出（数据顺序为：实际速度、目标速度、kp、ki、kd）
	ch_data[0] = pSpeedPID->sPID.pre;
	ch_data[1] = pSpeedPID->sPID.tar;
	
	ch_data[2] = pSpeedPID->sPID.kp;
	ch_data[3] = pSpeedPID->sPID.ki;
	ch_data[4] = pSpeedPID->sPID.kd;
	VOFA_JUSTFLOAT_SEND(ch_data, sizeof(ch_data)/sizeof(float));
}

